package edu.wpi.first.wpilibj.defaultCode;

import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.Victor;

/**
 *
 * @author Wesley
 */
public class DriveTrain
{
    private Encoder flDriveMtrEnc;
    private Encoder frDriveMtrEnc;
    private Encoder rlDriveMtrEnc;
    private Encoder rrDriveMtrEnc;

    private Encoder flRrSteerEnc;
    private Encoder frRlSteerEnc;

    private Victor flDriveMtr;
    private Victor frDriveMtr;
    private Victor rlDriveMtr;
    private Victor rrDriveMtr;

    private Victor flRrSteerMtr;
    private Victor frRlSteerMtr;

    private PIDController frRlSteerPID;
    private PIDController flRrSteerPID;

    private PIDController flMtrPID;
    private PIDController frMtrPID;
    private PIDController rlMtrPID;
    private PIDController rrMtrPID;

    private WheelState wheelStates;

    public DriveTrain()
    {
        System.out.println("DriveTrain::DriveTrain");
        flDriveMtrEnc = new Encoder(1, Utils.FL_DRIVE_ENC, Utils.DRIVE_ENC_NUM_TICKS);
        frDriveMtrEnc = new Encoder(1, Utils.FR_DRIVE_ENC, Utils.DRIVE_ENC_NUM_TICKS);
        rlDriveMtrEnc = new Encoder(1, Utils.RL_DRIVE_ENC, Utils.DRIVE_ENC_NUM_TICKS);
        rrDriveMtrEnc = new Encoder(1, Utils.RR_DRIVE_ENC, Utils.DRIVE_ENC_NUM_TICKS);

        flRrSteerEnc = new Encoder(1, Utils.FL_RR_STEER_ENC, Utils.STEER_ENC_NUM_TICKS);
        frRlSteerEnc = new Encoder(1, Utils.FR_RL_STEER_ENC, Utils.STEER_ENC_NUM_TICKS);

        flDriveMtr = new Victor(Utils.FL_DRIVE_MOTOR);
        frDriveMtr = new Victor(Utils.FR_DRIVE_MOTOR);
        rlDriveMtr = new Victor(Utils.RL_DRIVE_MOTOR);
        rrDriveMtr = new Victor(Utils.RR_DRIVE_MOTOR);

        wheelStates = new WheelState();

        setupPID();
    }

    /**
     * Given the input values, calculate how each of the wheels is supposed to
     * move and set the PID controllers appropriately
     * @param xVal x-value of the joystick vector in degrees
     * @param yVal y-value of the joystick vector in degrees
     * @param twist twist magnitude
     */
    public void driveTo(double xVal, double yVal, double twist)
    {
        System.out.println("DriveTrain::driveTo(xVal, yVal, twist)");
        double vMag = Math.sqrt(xVal*xVal + yVal*yVal);

        if(Math.abs(twist) <= .1)
        {
            double angle = Utils.calculateAng(xVal, yVal);
            double currAngle = flRrSteerEnc.getAngle();

            //Scale the magnitude if it is more than 1
            if(vMag > 1)
            {
                vMag = 1;

            }

            double checkAngle = getRotaryShortestPath(currAngle, angle);

            //If there is a shorter path for the wheels, swap the sign on the
            //motors
            if(checkAngle != angle)
            {
                wheelStates.FL_SIGN *= -1;
                wheelStates.FR_SIGN *= -1;
                wheelStates.RL_SIGN *= -1;
                wheelStates.RR_SIGN *= -1;
            }

            frRlSteerPID.setSetpoint(angle);
            flRrSteerPID.setSetpoint(angle);

            flMtrPID.setSetpoint(wheelStates.FL_SIGN*vMag);
            frMtrPID.setSetpoint(wheelStates.FR_SIGN*vMag);
            rlMtrPID.setSetpoint(wheelStates.RL_SIGN*vMag);
            rrMtrPID.setSetpoint(wheelStates.RR_SIGN*vMag);
        }
        else  
        {
            //Move wheels to turn position  /     \
               //                           \      /
            //                               -     +
            //                               -     +
        }


    }

    /**
     * Sets the appropriate PID controller values to drive the robot as fast as
     * possible to the given angle
     * @param angle Angle to drive to
     */
    public void driveTo(double angle)
    {
        System.out.println("DriveTrain::driveTo(angle)");
    }

    /**
     * Initialize the PID controllers for the wheel speed and rotation
     */
    private void setupPID()
    {
        System.out.println("DriveTrain::setupPID");
        frRlSteerPID = new PIDController(Utils.P_STEER_CONST, Utils.I_STEER_CONST, Utils.D_STEER_CONST, frRlSteerEnc, frRlSteerMtr, .005);
        flRrSteerPID = new PIDController(Utils.P_STEER_CONST, Utils.I_STEER_CONST, Utils.D_STEER_CONST, flRrSteerEnc, flRrSteerMtr, .005);
        
        flMtrPID = new PIDController(Utils.P_DRIVE_CONST, Utils.I_DRIVE_CONST, Utils.D_DRIVE_CONST, flDriveMtrEnc, flDriveMtr, .005);
        frMtrPID = new PIDController(Utils.P_DRIVE_CONST, Utils.I_DRIVE_CONST, Utils.D_DRIVE_CONST, frDriveMtrEnc, frDriveMtr, .005);
        rlMtrPID = new PIDController(Utils.P_DRIVE_CONST, Utils.I_DRIVE_CONST, Utils.D_DRIVE_CONST, rlDriveMtrEnc, rlDriveMtr, .005);
        rrMtrPID = new PIDController(Utils.P_DRIVE_CONST, Utils.I_DRIVE_CONST, Utils.D_DRIVE_CONST, rrDriveMtrEnc, rrDriveMtr, .005);

        //***********************************************
        //These will probably need to be different for the differnt types of
        //encoders. Tune this!
        frRlSteerPID.setTolerance(Utils.PID_TOLERANCE);
        flRrSteerPID.setTolerance(Utils.PID_TOLERANCE);
        
        flMtrPID.setTolerance(Utils.PID_TOLERANCE);
        frMtrPID.setTolerance(Utils.PID_TOLERANCE);
        rlMtrPID.setTolerance(Utils.PID_TOLERANCE);
        flMtrPID.setTolerance(Utils.PID_TOLERANCE);
        //***********************************************

        //***********************************************
        frRlSteerPID.setInputRange(0, 360);
        frRlSteerPID.setInputRange(0, 360);

        flMtrPID.setInputRange(0, 360);
        frMtrPID.setInputRange(0, 360);
        rlMtrPID.setInputRange(0, 360);
        rrMtrPID.setInputRange(0, 360);
        //***********************************************

        //***********************************************
        frRlSteerPID.setContinuous();
        frRlSteerPID.setContinuous();

        flMtrPID.setContinuous();
        frMtrPID.setContinuous();
        rlMtrPID.setContinuous();
        rrMtrPID.setContinuous();
        //***********************************************

        //***********************************************
        frRlSteerPID.enable();
        frRlSteerPID.enable();

        flMtrPID.enable();
        frMtrPID.enable();
        rlMtrPID.enable();
        rrMtrPID.enable();
        //***********************************************

    }

    /**
     * RotaryShortestPath()
     *
     * Returns the shortest delta path from current angle to target angle.
     *
     */
    private double getRotaryShortestPath(double target, double current )
    {
        System.out.println("DriveTrain::getRotaryShortestPath");
        double output = target - current;

        output = output % 360.0;

        if( output < -180.0 )
            output = 360.0 + output;
        if( output > 180.0 )
            output = output - 360.0;

        return output;
    }

    /**
     * Realign wheels to zero
     */
    public void zeroWheels()
    {

    }
}
